#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     HTAC,               sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  motorA,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorB,          ,              tmotorNormal, openLoop, encoder)
#pragma config(Motor,  motorC,          legomc,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     a,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     b,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     c,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     d,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     e,             tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     f,             tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    serva,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/CompleteDriver.h"

#define YTARGET (_y_axis == 0)
#define OFF 0
#define ONF 1
#define ONB 2
#define COMPLETE 3

#define DEBUG

int state = OFF;

int _y_axis = 0;

void yerror () {
  if (_y_axis > -20 && _y_axis < 20) {
    _y_axis = 0; }
}


int timetostop;
int timetocenter;
int movespeed;

int encocertostop;
int encodertocenter;

task main {
  StartTask(sensorRead);
  calibrateAccel();
  bDisplayDiagnostics = false;
  int i;
#ifdef DEBUG
  writeDebugStreamLine("STARTING");
#endif
  //StopTask (displayDiagnostics);
  /*HTSMUXinit();
  HTSMUXscanPorts(SMUX_1);
  HTSMUXscanPorts(SMUX_2);
  wait1Msec(500);*/
  timetocenter = 500;
  encodertocenter = 170;
  timetostop = 100;
  movespeed = 21;
  crossBridge(true, 3000, 30,false);
  wait1Msec(timetostop);
  writeDebugStream("------STAGE2START------");
  while (true){
    _y_axis = AccelReadingY();
    yerror();
#ifdef DEBUG
    writeDebugStreamLine("DISPLAYING");
    writeDebugStreamLine("Ay %d", _y_axis);
#endif
    while (_y_axis != 0){
      _y_axis = AccelReadingY();
      yerror();
#ifdef DEBUG
      writeDebugStreamLine("DISPLAYING");
      writeDebugStreamLine("Ay %d", _y_axis);
#endif
      if (state == COMPLETE){
        timetocenter = 150;
        encodertocenter = 29;
        timetostop = 1900;
        movespeed = 30;
      }
      if  (_y_axis < 0){
        if (state == OFF){
          state = ONF;
        }
        if (state == ONB){
          state = COMPLETE;
          for (i = 0; i < 1; i++){
            crossBridge(false, encodertocenter, movespeed,false);
            //backward (movespeed);
            //wait1Msec(timetocenter);
            stopmot();
            wait1Msec(timetostop);
          }
          continue;
        }

        crossBridge(false, encodertocenter, movespeed,false);
        //backward(movespeed);
        //wait1Msec(timetocenter);
        stopmot();
        wait1Msec(timetostop);
      }
      if (_y_axis > 0){
        if (state == OFF){
          state = ONB;
        }
        if (state == ONF){
          state = COMPLETE;
          for (i = 0; i < 1; i++)
            crossBridge(true, encodertocenter, movespeed,false);
          //forward(movespeed);
          //wait1Msec(timetocenter);
          stopmot();
          wait1Msec(timetostop);
          continue;
        }
        crossBridge(true, encodertocenter, movespeed,false);
        //forward(movespeed);
        //wait1Msec(timetocenter);
        stopmot();
        wait1Msec(timetostop);
      }
    }
  }
}
